三维空间坐标系变换——旋转矩阵

空间中三维坐标变换一般由三种方式实现，第一种是旋转矩阵和旋转向量；第二种是欧拉角；第三种是四元数。这里先介绍旋转矩阵(旋转向量)与欧拉角实现三维空间坐标变换的方法以及两者之间的关系。

       这里以常见的世界坐标系与相机坐标系间的变换为例。

一、首先介绍从相机坐标系转换到世界坐标系，也就是比较通用的body到世界坐标系间的转换。

       那么旋转的欧拉角按从世界坐标系转换到相机坐标系的过程，先按z轴旋转、之后y轴旋转、之后x轴旋转，最终得到相机坐标系，得到的角度分别是yaw、pitch、roll，那么从相机坐标系到世界坐标系的旋转矩阵按如下方式定义:

那么得到的相机（body）坐标系到世界坐标系间的旋转矩阵为：

cv::Mat IMUReader::angleToRotation(COORDINATES& carrier)
{
// R
cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
R.at<float>(0, 0) = cos(carrier.yaw)*cos(carrier.pitch);
R.at<float>(0, 1) = cos(carrier.yaw)*sin(carrier.roll)*sin(carrier.pitch) - cos(carrier.roll)*sin(carrier.yaw);
R.at<float>(0, 2) = sin(carrier.yaw)*sin(carrier.roll) + cos(carrier.yaw)*cos(carrier.roll)*sin(carrier.pitch);
R.at<float>(1, 0) = cos(carrier.pitch)*sin(carrier.yaw);
R.at<float>(1, 1) = cos(carrier.yaw)*cos(carrier.roll) + sin(carrier.yaw)*sin(carrier.roll)*sin(carrier.pitch);
R.at<float>(1, 2) = cos(carrier.roll)*sin(carrier.yaw)*sin(carrier.pitch) - cos(carrier.yaw)*sin(carrier.roll);
R.at<float>(2, 0) = - sin(carrier.pitch);
R.at<float>(2, 1) = cos(carrier.pitch)*sin(carrier.roll);
R.at<float>(2, 2) = cos(carrier.roll)*cos(carrier.pitch);

return R;
}

对于平移矩阵为相机坐标系原点在世界坐标系下的坐标T，最终得到的坐标变换方程如下：

其理解过程为从世界坐标系经过z，y，x的旋转以及平移T后得到相机坐标系，那么从相机坐标转换的世界坐标实际上是反变换过程，最后一步是x旋转，那么通过定义Rx为反旋转将相机坐标反旋转过来，之后依次是y，z，旋转后得到的值为与世界坐标系同方向，但是有一个平移的T的坐标系下的坐标，之后再加一个T坐标就得到了在世界坐标系下的坐标。

 

二、下面介绍从世界坐标系旋转到相机坐标系，这是一个正向旋转的过程，首先绕z，y，x旋转并平移T得到相机坐标系，那么从世界坐标系到与相机坐标系同向的坐标系是只经过旋转，那么旋转矩阵为：

RxRyRz，而此时三个维度的旋转分别是：

 

 

对于平移矩阵是，此时坐标已经是与相机坐标系同向，但是与世界坐标系原点重合的坐标下的坐标，转换相机坐标系还需要加上的平移是世界坐标系原点在相机坐标系下的坐标T。

其理解过程就是一个在世界坐标系依照顺序进行z，y，x的旋转，之后再平移的过程。

其中对于z，y，x方向的旋转都是按照右手系定则，大拇指指向轴线方向，四指方向为旋转的正方向。

 

下面为之前整理，可能有错误，仅借鉴。

界坐标系下的坐标（Xw,Yw,Zw）转换到相机坐标系下（Xc，Yc,Zc）。

     那么这里涉及到旋转矩阵和平移矩阵。对于旋转我们有欧拉角描述，根据欧拉角计算出旋转矩阵。对于欧拉角指的是坐标系绕某个轴旋转的角度，那么从一个坐标系旋转到另一个坐标系需要三次旋转能够完成。对于绕坐标轴旋转分为两种，一种是绕原坐标系的固定轴旋转，一种是绕部分旋转后的坐标轴旋转，这里介绍比较常用的绕部分旋转后的坐标轴旋转。同时，对于绕坐标轴旋转其旋转的顺序也是有关系，如绕XYZ旋转和绕ZYX旋转。那么一种比较常用的表示方式是航空航天的偏航-俯仰-滚转表示法，其中坐标可以沿机头朝向为x轴，飞机左侧为y轴，飞机正上方为z轴。那么偏航为绕z轴旋转，之后俯仰是绕y轴旋转，滚转是绕x轴旋转，即该欧拉角的旋转顺序为ZYX.

       上面介绍了旋转方式-欧拉角，下面介绍旋转矩阵的生成。这里分开不同轴讨论，先绕X轴旋转。我们定义旋转变换公式为：

其中，M为旋转矩阵，公式定义的是从世界坐标系向相机坐标系的转换，那么我们就按世界坐标向相机坐标旋转，如下：

这里我们定义了从左侧的世界坐标转到右侧的相机坐标，其旋转为绕X轴正向旋转90度，这里的正向旋转是以沿X轴方向看，顺时针旋转为正向，否则为负向。那么从世界坐标系向相机坐标系的旋转矩阵即为：

此时从世界坐标向相机坐标的转换过程如下：

所以，这里需要注意的是几点：（1）坐标是从世界坐标系向相机坐标系旋转，所有对于旋转方程是旋转矩阵乘以世界坐标得到相机坐标；（2）旋转是从世界坐标向相机坐标的旋转，即从源坐标系向目标坐标系旋转，这里是从世界坐标系向相机坐标系旋转；（3）旋转角度是沿着坐标轴的正向看，顺时针为正，逆时针为负；（4）旋转矩阵的形式如上，绕坐标轴旋转的轴坐标是不会变化的，所以是对应位置是1，接下来是0 cos(theta)  sin(theta), 在接下来是0   -sin(theta)  cos(theta)。

以上四个点的方向是与最终的变换公式相关的，有一个地方发生变化，那么最终的旋转变换公式也会发生变化，为了统一，那么这里按这样的规定，从而确定旋转变换公式的固定。

        上面介绍了绕X轴旋转的情况，其它绕Y轴和Z轴的旋转矩阵相同，分别是：

绕Y轴旋转矩阵：

绕Z轴旋转矩阵：

那么假如从世界坐标系向相机坐标系的旋转过程中，显示绕Z轴旋转了theta，之后绕旋转后坐标系的Y轴旋转了beta，之后绕旋转后坐标系X轴旋转了alpha，那么最终的旋转矩阵如下：

以上是对旋转矩阵的描述，那么对三维坐标系的转换，除了包含旋转，还要包含平移。即坐标系先从源旋转到与目标坐标系相同的方向，之后再平移。对于平移这里也规定从源坐标向目标坐标系。

这里目标坐标系在源坐标系下的坐标是(1,3,2),那么从源坐标系转到目标坐标系的变换即为：

 

于是这里规定几点：（1）变换是从源坐标系到目标坐标系，这里是从世界坐标系到相机坐标系；（2）平移变换是目标坐标系原点在源坐标系旋转到与目标坐标系相同方向后的坐标系下的坐标，这里即变换矩阵是目标坐标系原点在世界坐标系变换为与目标坐标系相同方向后在变换后世界坐标系中的坐标位置；（3）此时的变换矩阵需要加上负号。

如果不加负号，也可以将变换矩阵定义为源坐标系原点在目标坐标系下的位置，这里不用变换源坐标系。经过以上步骤可以将源坐标系中的坐标点转换到目标坐标系下。

 

对于旋转矩阵的旋转向量以及欧拉角的关系与变换：

对于旋转矩阵与旋转向量：

上面介绍了通过旋转矩阵和平移矩阵描述两个坐标系间的变换，其实也相当于表示了6自由度的三维刚体运行。但是旋转矩阵有几个缺点：SO(3)的旋转矩阵有9个量，但一次旋转只有3个自由度。因此，这种表达式冗余。而且对于旋转矩阵自身也有约束，它必须是正交矩阵，且行列式为1，这些约束会使求解变得困难。一种更紧凑的方式是使用旋转xian向量描述。即对于坐标系的旋转，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴yizh一致，而长度等于旋转角。这种向量称为旋转向量(或轴角，Axis-Angle)。这种表示法只需一个三维向量即可描述旋转。对于xuan旋转向量和旋转矩阵间的转换可以通过罗德里格斯公司（Rodrigues Formula）来计算。

对于旋转矩阵与欧拉角：